/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef __MURO_MACHINE_HPP__
#define __MURO_MACHINE_HPP__

#include "PositionClient.hpp"
#include "RangingClient.hpp"
#include "AlarmClient.hpp"
#include "EnergyClient.hpp"
#include "MapClient.hpp"
#include "SpeedCtrlClient.hpp"
#include "CoordinateDriveClient.hpp"

#include "RangingDisplay.hpp"
#include "picking/picking.hpp"
#include "GLDisplay.hpp"

#include "MuRoMainFrame.h"

#include "Devices.hpp"

#include <wx/event.h>
#include <wx/menu.h>

//TODO: these to settings...
#define UNIT_COLOR_SELECTED 1.0, 1.0, 0.0
#define UNIT_COLOR_DESELECTED 0.7, 0.7, 0.0
#define LINE_WIDTH_SELECTED 2
#define LINE_WIDTH_DESELECTED 1


namespace MuRo
{

class CDevice;
  
class CMuRoCore;
  
enum EMachineDriveMode
{
  KDriveModeAuto    = 1,
  KDriveModeManual  = 2
};

enum EOperationMode
{
  KOperModeObservation  = 0, //no control over robot, just observation
  KOperModeManualDrive  = 1,
  KOperModeCoordDrive   = 2,
  KOperModeSetPosition  = 3
  //KOperModeMapEdit = 4, etc...
};

enum EMachineStatus
{
  KStatusIdle = 0,
  KStatusWaiting = 1,
  KStatusDriving = 2
};

typedef std::vector<CDevice*> deviceVector;
typedef std::vector<deviceVector*> deviceTree;


class CMachine: public wxEvtHandler,
//                 public MaCI::CoordinateDrive::CCoordinateDriveClientCallback,
                public CGLPickable
{
  //DECLARE_EVENT_TABLE()
  friend class CDevice;
  
  public:
    CMachine(std::string name, MuRoMainFrame *MuRo);
    
    void OnMenuEvent(wxCommandEvent &e);
    
    
    void SetViewParams(const TViewParams &view);
    
    void Draw();
    
    void DrawGhost(float x, float y, float heading);
    
    void AddPositionClient(MaCI::Position::CPositionClient* positionClient);
    void AddRangingClient(MaCI::Ranging::CRangingClient* rangingClient);
    void AddAlarmClient(MaCI::Alarm::CAlarmClient* alarmClient);
    void AddMapClient(MaCI::Map::CMapClient* mapClient);
    void AddEnergyClient(MaCI::Energy::CEnergyClient* energyClient);
    void AddSpeedCtrlClient(MaCI::SpeedCtrl::CSpeedCtrlClient* speedCtrlClient);
    void AddCoordinateDriveClient(MaCI::CoordinateDrive::CCoordinateDriveClient* coordDriveClient);
    
    //try to reset position interfaces position
    bool SetPose(float x, float y, float heading);
    
    void Select(bool selected);
    bool IsSelected() const;
    
    std::string GetName() const;
    
    
    void SetOperationMode(EOperationMode mode);
    
    //int SetDriveMode(EMachineDriveMode mode) {driveMode=mode;}
    
    int GetStatus() {return status;}
    //int GetDriveMode() {return driveMode;}
    
    int GetOperationMode() {return operationMode;}
    
    float GetPositionX();// {return positionX;}
    float GetPositionY();// {return positionY;}
    float GetHeading();// {return heading;}
    
    //call these to drive machine
    void Drive(float power, float steering);
    
    void AddPathPoint(float x, float y);
    void DriveToPoint(float x, float y);
    void StartDrivingPath();
    void StopDriving();
    void RemoveSelectedPathPoints();
    
    bool HasManualDrive();
    bool HasCoordDrive();
    bool HasPosition();
    
    void SetReferencePosition(CPositionDevice *position);
    CPositionDevice *GetReferencePosition();
    void SetSpeedCtrlDevice(CSpeedCtrlDevice *speedCtrl);
    CSpeedCtrlDevice *GetSpeedCtrlDevice();
    void SetCoordinateDriveDevice(CCoordinateDriveDevice *coordDrive);
    CCoordinateDriveDevice * GetCoordinateDriveDevice();
    
    void SetPositionHistoryLengthS(int length);
    void ShowPositionHistory(bool history);
    bool IsPositionHistoryShown();
    
    void SetRangingHistoryLengthS(int length);
    void ShowRangingHistory(bool history);
    bool IsRangingHistoryShown();
    
    /* picking */
    void DrawPickable();
    
    wxMenu* GetMenu();
    
    wxTreeItemId GetTreeItem(){return treeItem;}
    
    void SetTreeItem(wxTreeItemId item){treeItem=item;}
    
    deviceTree& GetDeviceTree();

  // Machine device mutex functions
  void LockDeviceMutex();
  void UnlockDeviceMutex();
    
  private:
    
    MuRoMainFrame *MuRo;
    bool selected;
    bool visible;
    
    wxMenu menu;
    
    wxTreeItemId treeItem;
    
    TViewParams view;
    
  EOperationMode operationMode;
  EOperationMode iPreviousOperationMode; ///< This is the mode that was used before KOperModeSetPosition
//     EMachineDriveMode driveMode;
    EMachineStatus status;
    
    std::string name;
    CPositionDevice *referencePosition;
    CSpeedCtrlDevice *speedCtrl;
    CCoordinateDriveDevice *coordinateDrive;
    
    deviceVector positions;
    deviceVector rangings;
    deviceVector alarms;
    deviceVector maps;
    deviceVector energys;
    deviceVector speedCtrls;
    deviceVector coordDrives;
        
    deviceTree devices;
    
    void drawDevices();
    
    
};

}

#endif
